Yes — an Extended Kalman Filter (EKF) or Unscented Kalman Filter (UKF) can be set up to detect wheel or track slip using an IMU, but only if you give the filter the right model and measurements to reveal the inconsistency.

How it works

1. Typical setup in mobile robotics

• Inputs: Wheel/track odometry (speed or displacement from encoders) + IMU data (linear acceleration, angular velocity).

• Filter's role: Fuse these to estimate the platform's position, velocity, and orientation.

• Slip detection idea: If the filter’s motion model predicts one thing and the measurements disagree systematically, the residual (innovation) will spike — this is a sign of slip.

2. Why slip shows up in an EKF/UKF

• Normal driving:

  • Wheel odometry and IMU agree on acceleration and yaw rate.

• Slip event:

  • Wheel odometry indicates motion (e.g., forward speed), but the IMU shows smaller acceleration or angular velocity (or vice versa).

  • This mismatch causes measurement innovations to grow, which the filter will correct for.

• If you monitor innovation magnitude or covariance, you can flag slip.

3. Model features that help detect slip

• Process model: Captures vehicle kinematics from IMU.

• Measurement model: Maps wheel encoder readings to predicted velocity/pose.

• Slip representation:

  • Either implicit: rely on residual spikes.

  • Or explicit: add slip ratio or slip velocity as extra states in the filter to estimate it in real time.

4. EKF vs UKF in this context

• EKF: Works fine if your slip model is linearizable and the state space is not too nonlinear.

• UKF: Often handles strong nonlinearities better — useful if you directly model slip coefficients or have high lateral slip angles.

• Both can detect slip; UKF just gives you cleaner estimates in heavily nonlinear regimes.

5. Practical approach

• Without explicit slip state:

  • Monitor innovation statistics (from either the odometry or IMU update steps). Persistent bias means likely slip.

• With explicit slip state:

  • Add longitudinal and lateral slip ratios into the state vector.

  • Use IMU + odometry to estimate them in real time.

Example signal of slip detection in EKF:

• You predict forward velocity v_pred from IMU integration.

• Encoder-based measurement v_enc says you're going 2 m/s.

• Residual: v_enc - v_pred ≈ 1.5 m/s (large, persistent) → likely slip.

If you want, I can sketch a minimal EKF state model with slip as an extra state, so you could estimate it explicitly rather than just detect it. That way, your filter not only spots slip but also quantifies how much. Would you like me to do that?

Short answer: yes. In Gazebo with the TrackedVehicle controller (steering_efficiency=0.5), the track odometry you get from encoders will systematically disagree with the body motion sensed by the IMU (especially yaw rate). You can (a) detect that slip in real time from the IMU–encoder residuals, and (b) keep /odom smooth while continuously “realigning” it to world truth using a dual-EKF setup (standard in ROS with robot_localization).

Here’s a practical recipe that works well in sim:

1) Detect slip (IMU vs encoder residuals)

For tracked/diff drive kinematics,

• Encoder-implied yaw rate: ω_enc = (v_r − v_l) / b (track width b)

• IMU yaw rate: ω_imu = gyro_z

Large, persistent residuals mean slip:

pgsql
CopyEdit
r_ω = ω_enc − ω_imu
r_v = v_enc − v_imu   (v_imu from IMU-integrated velocity or from body accel after gravity removal)
if |r_ω| > k * sqrt(σ_ω^2_enc + σ_ω^2_imu)  for N consecutive samples → slip flag

Tip: with steering_efficiency=0.5 you’ll typically see |ω_enc| > |ω_imu| during turns (encoder overestimates rotation). Calibrate k (e.g., 3.0) and an “N” window (e.g., 10–20 samples at 100 Hz).

Optional: add slip states to the filter:

• Longitudinal slip ratio s_long and lateral slip s_lat, or a simple “steering efficiency” state η where ω_true = η * ω_enc. Let the EKF/UKF estimate η online; in sim it will converge near 0.5 under steady turning.

2) Keep /odom smooth but continuously realign to world (dual EKF)

Use two ekf_localization_node instances:

• EKF #1 (odom): fuse encoders + IMU only. Publishes odom → base_link. This frame must be smooth and never jump; it’s what controllers use.

• EKF #2 (map): fuse the same sources plus a world-referenced pose (in Gazebo you have ground truth from /gazebo/model_states or gazebo_ros_p3d). This publishes map → odom so that global drift is corrected without discontinuities in odom → base_link.

Minimal YAML sketch (tune covariances for your sim):

yaml
CopyEdit
# ekf_odom.yaml
frequency: 100
two_d_mode: true            # if your sim is planar
base_link_frame: base_link
odom_frame: odom
world_frame: odom
publish_tf: true

# encoders as Twist (vx, wz) in base_link
odom0: /wheel/odom
odom0_config: [false, false, false,
               false, false, true,   # roll/pitch/yaw? (if provided)
               true,  false, false,  # vx
               false, false, true]   # wz
odom0_differential: false

imu0: /imu/data
imu0_config: [true,  true,  false,   # roll, pitch, (no yaw from orientation to avoid mag bias)
              false, false, true,    # angular velocities (use wz)
              true,  true,  false]   # linear accel x,y (if you want velocity aiding)
imu0_remove_gravitational_acceleration: true

yaml
CopyEdit
# ekf_map.yaml
frequency: 50
two_d_mode: true
base_link_frame: base_link
odom_frame: odom
map_frame: map
world_frame: map
publish_tf: true
publish_acceleration: false

# same encoder + IMU as soft constraints
odom0: /wheel/odom
odom0_config: [false, false, false, false, false, false,
               true,  false, false, false, false, true]

imu0: /imu/data
imu0_config:  [true, true, false, false, false, true,
               true, true, false]
imu0_remove_gravitational_acceleration: true

# world-referenced pose from Gazebo (perfect ground truth)
pose0: /gazebo_ground_truth   # e.g., from gazebo_ros_p3d
pose0_config: [true, true, true,
               false, false, true,
               false, false, false,
               false, false, false]
pose0_differential: false
pose0_relative: false

Launch both EKFs; ekf_map will publish a drifting-correcting map→odom. Your controllers still use odom→base_link (smooth), while global consumers (e.g., mapping, logging) use map→base_link (drift-free).

3) “Re-aligning odom” on demand

If you need an explicit “snap back” (e.g., after getting stuck and slipping a lot), you have two clean options:

• Preferred (no jumps in odom): Leave odom→base_link alone. Publish a corrected map→odom via ekf_map (as above). This effectively recenters global pose without shocking controllers.

• If you must reset odom: call robot_localization’s /set_pose on the odom EKF with a new pose for base_link in odom. This will jump the TF and can upset controllers—only do it when paused or at low speed.

Example (rosservice):

css
CopyEdit
rosservice call /ekf_odom/set_pose "{pose: {header: {frame_id: 'odom'}, pose: {position: {...}, orientation: {...}}}}"

4) Simple online slip monitor (ROS nodelet idea)

• Subscribe: /wheel/odom (vx, wz), /imu/data (gyro_z).

• Compute residuals r_ω, r_v; low-pass filter and compare to thresholds.

• When slip is high, either:

  • Down-weight encoder covariances sent to EKF (inflate R online), or

  • Temporarily disable encoder updates (measurement timeout), letting IMU dominate.

5) Gazebo-specific tips

• Set realistic IMU noise in the plugin; otherwise the residual test will be too sensitive.

• If your encoder topic is derived from commanded joint velocities (not joint motion), switch to joint position/velocity sensors so that slip actually shows up.

• That steering_efficiency=0.5 effectively means the kinematic model over-predicts turn rate; expect a consistent bias in r_ω during turns—good for detection and for estimating an online efficiency state η.

If you want, I can share a small ROS node (Python) that subscribes to /wheel/odom and /imu/data, computes residuals, and toggles the EKF’s encoder measurement covariance dynamically when slip is detected.

Short answer: it doesn’t block you. You don’t strictly need the controller to publish real-time Twist; you just need two independent motion sources so your filter can (a) spot inconsistency (slip) and (b) realign drift. You can generate what you need in Gazebo:

What to use instead of the controller’s velocity

1. Joint states → pseudo-odometry (encoders)

• Read the sprocket joint velocities from /joint_states.

• Compute track linear speeds: v_l = r * ω_l, v_r = r * ω_r (sprocket radius r).

• Convert to body twist (your “encoder” measurement):

  • Linear: v_x_enc = (v_r + v_l)/2

  • Yaw rate: w_z_enc = (v_r - v_l)/b_eff

• Publish that as a nav_msgs/Odometry (or geometry_msgs/TwistWithCovarianceStamped) topic for your EKF. This is exactly what many diff/tracked setups do; the controller doesn’t need to provide it.

2. IMU is already giving angular velocity

• Use gyro_z directly; it’s your second, independent source vs. w_z_enc.

• Optionally integrate IMU linear accel (with gravity removal) to get v_x_imu (noisy but useful for residuals when filtered).

3. Gazebo ground truth for realignment (map→odom)

• Use /gazebo/model_states or gazebo_ros_p3d (pose in world). Feed this only to your map EKF so it can publish map→odom. That cleanly “realigns” without jumping odom→base_link.

Slip detection without controller Twist

• Compute residuals between pseudo-odometry (from joint states) and IMU:

  • r_w = w_z_enc − w_z_imu

  • r_v = v_x_enc − v_x_imu (or compare a_x vs. dv/dt from enc)

• Low-pass + threshold over a short window; when high, either:

  • Inflate the encoder covariance you feed to the EKF, or

  • Temporarily stop applying the encoder update so IMU dominates.

Minimal ROS wiring

• Node A: /joint_states → compute (v_x_enc, w_z_enc) → publish /wheel/odom (Twist or Odometry) with reasonable covariances.

• EKF #1 (“odom”): fuse /wheel/odom + /imu/data → publishes odom→base_link (smooth).

• EKF #2 (“map”): fuse /wheel/odom + /imu/data + /gazebo_ground_truth_pose → publishes map→odom (drift correction).

• Optional Node B: slip monitor that watches r_w, r_v and dynamically adjusts the encoder measurement covariance sent to EKF #1.

Notes & gotchas

• Use the sprocket joints (or whatever drives the tracks), not commanded velocities, so slip is observable.

• Choose an effective baseline b_eff that matches your Gazebo steering_efficiency (you can even estimate an online scale η so w_true = η·w_enc).

• Set realistic IMU noise in Gazebo; otherwise every tiny mismatch will look like slip.

• If you truly lack joint velocities, you can differentiate joint positions (or even odom→base_link pose) with smoothing, but joint velocities are preferable.

Bottom line: the lack of built-in velocity feedback from the tracked controller doesn’t matter. Build a small “encoder” node from joint states + your IMU, and you’ll have everything needed to detect slip and keep /odom aligned via the dual-EKF setup. If you want, I can sketch the tiny ROS Python node that publishes /wheel/odom from /joint_states.

Totally doable—and you can make it behave like real hardware (slip, noise, delays, drift) so your EKF/UKF decisions in sim will transfer to the field. Here’s a compact, realistic recipe.

1) Make the physics and sensors “real”

Tracks & ground

• Use compliant friction: set per-surface mu/mu2 with modest anisotropy (e.g., rubber-on-concrete 0.8/0.7; dirt 0.6/0.5; wet 0.4/0.35).

• Enable contact damping/stiffness so starts/stops cause brief slip spikes.

• Keep steering_efficiency=0.5 (models lateral slip). Expect encoder-inferred yaw to overshoot IMU yaw in turns.

IMU (pick plausible numbers)

• Continuous-time gyro noise density ≈ 0.02–0.05 rad/s/√Hz

• Gyro bias RW ≈ 0.001–0.01 rad/s/√Hz

• Accel noise density ≈ 0.2–0.4 m/s²/√Hz

• Accel bias RW ≈ 0.02–0.05 m/s²/√Hz

• Add ~5–15 ms latency and 1–2 ms jitter.

Encoders (from joints)

• Quantize counts (e.g., 2048 CPR → sprocket angle steps).

• Add ±0.2–0.5% scale error and small latency (~5–10 ms).

• Don’t derive them from commanded velocity; use actual joint velocity/position so slip can exist.

2) Build “encoder odom” from joints (no controller Twist needed)

Publish a realistic odom (vx, wz) with covariance:

python
CopyEdit
# ROS1, Python: joint_states -> wheel/odom (vx,wz) with covariance
#!/usr/bin/env python3
import rospy, numpy as np
from sensor_msgs.msg import JointState
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Quaternion
from tf.transformations import quaternion_from_euler

class TracksOdom:
    def __init__(self):
        self.left = self.right = 0.0
        self.last_t = None
        self.r_sprocket = rospy.get_param('~sprocket_radius', 0.09)   # m
        self.baseline   = rospy.get_param('~track_width',     0.52)   # m (geometric)
        self.scale_err  = rospy.get_param('~scale_error',     0.003)  # ~0.3% encoder scale bias
        self.pub = rospy.Publisher('wheel/odom', Odometry, queue_size=10)
        rospy.Subscriber('joint_states', JointState, self.cb)

    def cb(self, js):
        t = js.header.stamp
        try:
            iL = js.name.index('left_sprocket_joint')
            iR = js.name.index('right_sprocket_joint')
        except ValueError:
            return
        wL = js.velocity[iL]
        wR = js.velocity[iR]
        # Convert to track tangential speeds with small scale bias
        vL = self.r_sprocket * wL * (1.0 + self.scale_err)
        vR = self.r_sprocket * wR * (1.0 - self.scale_err)
        vx = 0.5 * (vR + vL)
        wz_enc = (vR - vL) / self.baseline

        msg = Odometry()
        msg.header.stamp = t
        msg.header.frame_id = 'odom'
        msg.child_frame_id = 'base_link'
        # Pose unknown here; publish twist only
        msg.twist.twist.linear.x  = vx
        msg.twist.twist.angular.z = wz_enc
        # Realistic covariance (vx, wz on diagonal); inflate modestly
        cov_vx = 0.05**2      # (m/s)^2
        cov_wz = (3.0*np.pi/180.0)**2  # (rad/s)^2 ~ 3 deg/s
        msg.twist.covariance = [cov_vx,0,0,0,0,0,
                                0,1e6,0,0,0,0,
                                0,0,1e6,0,0,0,
                                0,0,0,1e6,0,0,
                                0,0,0,0,1e6,0,
                                0,0,0,0,0,cov_wz]
        self.pub.publish(msg)

if __name__ == '__main__':
    rospy.init_node('tracks_encoder_odom')
    TracksOdom()
    rospy.spin()

3) Detect slip like you would on a real robot (residuals + gating)

Use measured gyro vs. encoder yaw rate; add low-pass and a short window. When slip is high, inflate the encoder measurement covariance so the EKF leans on the IMU.

python
CopyEdit
# ROS1, Python: slip monitor that dynamically scales encoder covariance
#!/usr/bin/env python3
import rospy, numpy as np
from sensor_msgs.msg import Imu
from nav_msgs.msg import Odometry

ALPHA = 0.15   # LPF for residuals
THRESH = 3.0   # ~3-sigma gating

class SlipGate:
    def __init__(self):
        self.wz_imu = 0.0
        self.r_lpf = 0.0
        self.pub = rospy.Publisher('wheel/odom_adaptive', Odometry, queue_size=10)
        rospy.Subscriber('imu/data', Imu, self.cb_imu)
        rospy.Subscriber('wheel/odom', Odometry, self.cb_odom)

        # Nominal measurement std devs
        self.sig_wz_imu = np.deg2rad(1.0)     # rad/s
        self.sig_wz_enc = np.deg2rad(3.0)     # rad/s

    def cb_imu(self, m):
        self.wz_imu = m.angular_velocity.z

    def cb_odom(self, m):
        wz_enc = m.twist.twist.angular.z
        r = wz_enc - self.wz_imu
        self.r_lpf = (1-ALPHA)*self.r_lpf + ALPHA*r

        # Normalized residual (approx.)
        sig = np.sqrt(self.sig_wz_imu**2 + self.sig_wz_enc**2)
        z = abs(self.r_lpf)/sig

        # Copy & adapt covariance
        out = m
        cov = list(out.twist.covariance)
        if z > THRESH:
            # Inflate encoder yaw-rate covariance by factor (robustifying)
            factor = min(50.0, 3.0*z)  # cap to avoid singularities
            cov[35] = cov[35] * factor
        else:
            # Restore toward nominal
            cov[35] = max(cov[35]/1.2, (self.sig_wz_enc**2))
        out.twist.covariance = cov
        self.pub.publish(out)

if __name__ == '__main__':
    rospy.init_node('slip_gate')
    SlipGate()
    rospy.spin()

Feed wheel/odom_adaptive to your EKF instead of wheel/odom. This is analogous to robust measurement down-weighting on hardware.

4) EKF/UKF wiring that mirrors real deployments

• EKF #1 (odom): fuse wheel/odom_adaptive + IMU only → odom→base_link (smooth; never jumps).

• EKF #2 (map): add a realistic world reference, not Gazebo truth. Use whatever your real robot would use:

  • Indoors: LiDAR scan matching (AMCL/NDT/Cartographer/Hector), AprilTag/localization beacons.

  • Outdoors: visual odometry/VIO, GPS/RTK when available.

This way, testing in Gazebo still respects the same observability and drift patterns as real life. ekf_map publishes map→odom so your global pose stays consistent without shocking controllers.

5) Parameter notes that make it feel real

• Baseline vs. steering efficiency: keep the true geometric baseline in the encoder node; let the EKF reconcile that the real yaw is η·wz_enc (implicitly via residuals) rather than baking η into kinematics. Optionally add a state η∈(0,1) in a UKF if you want the filter to learn it online.

• 2D mode: if you’re planar, run EKF in two_d_mode but still fuse roll/pitch/accel to catch track-induced rocking and to keep gravity compensation sane.

• Delays: if you add sensor latency, consider robot_localization’s sensor_timeout and imu0_nodelay/odom0_nodelay options, or time-align topics (message_filters).

• Chi-square gating: you can compute rᵀS⁻¹r per measurement; drop outliers when > threshold to mimic robust front-ends.

• Encoder dropouts: occasionally drop encoder messages for 100–300 ms to see if your odom remains stable (IMU-only bridging), like on a loose connector.

6) Re-aligning odom the realistic way

• Preferred: keep odom→base_link continuous; let EKF #2 publish map→odom from your realistic world reference (scan matching / VO / GPS). That’s how field robots avoid jumps while correcting drift.

• Emergency reset: if you must reset odom, pause motion and call /ekf_odom/set_pose. Treat as “service visit”, not a normal operation.

If you drop these pieces in, you’ll see the same signatures you’d see on hardware: encoder-IMU yaw mismatches during turns, spikes on rough terrain or on slick surfaces, and your EKF smoothly leaning on the IMU when slip flares up. If you want, I can tailor the YAML for robot_localization to your exact topics and rates.

Short, honest answer: with only an IMU and encoders, you can detect slip and keep /odom stable, and you can reduce bias/drift a lot—but you cannot globally “re-align” to a fixed world frame (like map) without some absolute reference (GPS, range-based localization, vision, beacon, manual reset, etc.). You can still: (1) detect when encoders lie, (2) estimate/compensate steering efficiency/slip online, (3) clamp drift during stops (ZUPT), and (4) reset /odom manually at known poses. That’s how many field robots run when they’re “dead-reckoning only.”

Here’s a practical, realistic plan.

1) What is and isn’t observable

• Observable with IMU + encoders: body velocity, yaw rate, relative pose, gyro/accel biases (slowly), steering efficiency/slip relative to encoder kinematics.

• Not observable: absolute x/y/heading drift over time (no global truth). So you maintain a smooth odom→base_link, but any “re-alignment” is either (a) bias compensation, (b) zero-velocity corrections, or (c) a manual reset to a known pose.

2) Slip-aware EKF/UKF that actually helps

Use a planar EKF/UKF with these states (compact but effective):

ini
CopyEdit
x = [ px, py, ψ, vx, ωz, bg, ba_x, η ]^T

• ψ: yaw, vx: forward velocity, ωz: yaw rate

• bg, ba_x: gyro/accel biases (RW)

• η: steering efficiency (0–1), maps encoder yaw to true yaw: ω_true ≈ η·ω_enc

• (Optional) add left/right longitudinal slip s_L, s_R if you want per-track slip.

Process (IMU):

• Integrate ω_imu − bg, a_x_imu − ba_x, with gravity removal (2D).

• Biases follow random walks.

Measurements:

1. Encoders → Twist:
   v_enc = (v_R+v_L)/2 , ω_enc = (v_R−v_L)/b
   Measurement model:

   csharp
   CopyEdit
   z_enc = [ v_enc,
             ω_enc ]  with   h(x) = [ vx,
                                       η·ω_enc ]   # η estimated online
   

   (Equivalently, treat ω_imu as measuring η·ω_enc—both forms work.)

2. Nonholonomic constraint (NHC): lateral velocity in body ≈ 0 → add as a pseudo-measurement to suppress yaw/velocity drift during straight runs.

3. Zero-velocity updates (ZUPT): when both encoder speed and IMU acceleration norm drop below thresholds for ≥N samples, inject vx=0, ωz=0 with small covariance to clamp drift and re-zero biases.

Slip detection & robustness:

• Residuals: r_ω = (η·ω_enc) − ω_imu, r_v = v_enc − vx

• Gate with χ² or |r|/σ>k; when tripped, inflate encoder R so the filter leans on IMU until slip subsides.

• Because η is in the state, the filter will learn your Gazebo steering_efficiency≈0.5 during turns, cutting systematic yaw bias even when not slipping.

3) Wiring this in ROS (no global sensor)

• Publish encoder odom (Twist) from /joint_states (sprocket velocities), not commands.
  (You can use the small node I sketched earlier.)

• EKF (odom-frame only): fuse /imu/data + /wheel/odom + (optionally) a NHC pseudo-measurement and ZUPT events. Set world_frame: odom and do not publish any map frame.

• Add a tiny “slip gate” that scales the encoder covariance on the fly from the residuals (very realistic).

robot_localization tip if you don’t want to write your own UKF:

• Put /wheel/odom into odom0 (Twist only).

• Put /imu/data into imu0 (use gyro + accel, remove gravity).

• Enable two_d_mode: true.

• Implement NHC + ZUPT as occasional measurements: publish Twist messages with vy=0 (NHC) or vx=0, wz=0 (ZUPT) and small covariances only when the detector triggers. This approximates the constraints without custom code.

• You can approximate η by scaling ω_enc before publishing (cruder), or do it properly by adding a custom filter or a small UKF.

4) “Re-aligning /odom” without a global sensor

You can’t anchor to world, but you can still:

• Continuously re-align internally by estimating η and using ZUPT/NHC → this removes systematic turning bias and limits drift growth.

• Manual/tactical resets: when you reach a known dock/marker, call the EKF’s /set_pose to place base_link at a known odom pose (pause motion). This is what many dead-reckoning robots do.

• Consistent restarts: if you have a known start pose, reset odom at boot.

5) Realistic behavior knobs (both sim and field)

• Noise & bias: give the IMU realistic noise/bias RW; quantize encoders; add 5–15 ms latency. Tune R and Q so ZUPT/NHC actually move the state.

• Detection thresholds: start with 3σ gating over a 50–150 ms LPF window.

• Constraint cadence: don’t spam NHC/ZUPT; inject at 5–20 Hz when valid.

• Baseline: keep the geometric baseline in your encoder kinematics; let η account for steering/slip—this avoids double-counting.

6) If you want the “cleanest” result with only IMU+encoders

• Implement the UKF with η and NHC+ZUPT (10–12 state dimensions).

• Add the slip gate to down-weight encoders during detected slip.

• Keep only odom→base_link TF; don’t fabricate map.

If you’d like, I can give you:

• a minimal UKF state/measurement sketch (process & Jacobians), or

• robot_localization YAML + a tiny node that publishes NHC/ZUPT pseudo-measurements based on IMU/encoder triggers.

Which would you prefer?

Yes — if you have no global reference (GPS, vision, range-based localization, known markers, etc.), then once slip happens, the robot’s estimated pose will be wrong by however much the slip displaced it in the real world.

You can still reduce how wrong it gets, but you can’t magically know the true position afterward without an outside reference.

What actually happens in this case

• Slip detection just tells you, “My odometry is lying right now.”

• Without a map or GPS to compare against, you can’t know the exact error — only that an error exists.

• The EKF/UKF can limit further error growth by:

  • Down-weighting the bad encoder measurement (lean on IMU).

  • Estimating slip/steering-efficiency states to avoid systematic bias.

  • Using constraints like NHC (nonholonomic constraint) and ZUPT (zero velocity updates) to re-zero velocity and reduce drift.

• But the position estimate will still be shifted from reality by whatever displacement occurred during the slip.

Real-world analogy

Imagine you’re blindfolded, counting your steps and turns to navigate.
If you slip on ice for two steps, you know you slipped (from the feel), but unless someone tells you where you ended up, your internal map will be off by that amount forever — until you see a landmark.

Your options in a “IMU + encoders only” robot

• Live with the drift: Detect slip, limit damage (bias estimation, down-weighting), but accept that odom is only relative.

• Manual reset: When you reach a known location, reset odom to that spot.

• Add cheap landmarks: Even a couple of AprilTags or QR codes in your environment can give you a global “snap back” when detected.

If you want, I can show you how to modify your EKF so slip detection immediately stops feeding in bad encoder motion — that way, the drift from each slip is as small as possible, even though you can’t fully correct it. Would you like me to sketch that?

Short version: with only IMU + encoders, you can’t recover the true pose after slip (no external reference), but you can make the drift in that plot much smaller and keep it bounded over longer runs. The trick is to (a) stop trusting encoders during slip, (b) estimate/compensate the systematic turn bias (“steering efficiency”), and (c) use kinematic constraints and zero-velocity updates to continually pull the filter back to a self-consistent state.

Here’s the practical “fix what you can” list that works well on tracked robots:

1) Model the steering bias (η)

Slip in tracked steering shows up as encoder yaw rate being too large. Add a steering-efficiency state η∈(0,1) (or estimate it offline and apply as a scale):

• Encoders give ω_enc = (v_r − v_l)/b.

• In your measurement model use ω_true ≈ η · ω_enc.

• Let η be a slow random walk; the UKF/EKF will converge near your sim’s 0.5 and remove the systematic yaw drift that bends your blue/red curves away from GT.

(If you don’t want to touch the filter, pre-scale before publishing: ω_pub = η̂ · ω_enc with η̂ learned/calibrated.)

2) Robustify the encoder update during slip

Detect slip from the residual r_ω = ω_enc − ω_imu (LPF + 3σ gate). When flagged:

• Inflate the encoder R for ω (and optionally v) by 10–50×, or

• Temporarily drop the encoder update.
  This prevents the filter from “believing” bad odometry while the IMU carries you through the event.

3) Apply kinematic constraints

Two free, very effective constraints for ground robots:

• NHC (nonholonomic): body lateral velocity ≈ 0. Inject v_y = 0 as a pseudo-measurement at 5–20 Hz with small covariance (only when not turning sharply).

• ZUPT (zero-velocity update): when stopped (low encoder speed and IMU accel norm near g), inject v_x=0, ω_z=0. This re-zeros drift and helps estimate gyro/accel biases.

In robot_localization, you can implement NHC/ZUPT by occasionally publishing Twist messages with those components and tight covariances when the detector triggers.

4) Calibrate the encoders & baseline

Drift explodes if the wheel radius or baseline is off by a few percent.

• In sim: fit η, wheel radius scale, and baseline against Gazebo GT once; then freeze them and re-run without GT.

• In real life: do short, measured maneuvers (straight 10 m, in-place 360°) and solve for radius/baseline/η (least-squares).

5) Bias handling that actually helps

• Run a short stationary bias alignment at boot (5–10 s): estimate gyro bias and initial roll/pitch from gravity.

• Keep b_g and (longitudinal) b_a in the state as random walks; ZUPT/NHC will keep them bounded.

6) Time alignment & covariances

• Mis-timestamped IMU/encoders cause “mystery curvature.” Ensure synced clocks and compensate known latencies.

• Start with realistic R: gyro ~1–2 °/s (rad/s), encoder yaw ~2–4 °/s, encoder v_x ~0.05–0.1 m/s. Then tune Q so ZUPT/NHC actually pull.

What this buys you

• Absolute re-alignment? No (you’d need GPS/vision/UWB/landmarks).

• Relative drift reduction? Yes—often a 2–10× improvement on the trajectory like your plot: curves stop “bowing” during turns, straight segments stay straight, and stops clean up accumulated bias.

Minimal wiring (ROS, no new sensors)

• Publish encoder odom from /joint_states (tangential speeds → v_x, ω_enc).

• Slip gate node inflates encoder covariance when |ω_enc−ω_imu| is large.

• EKF/UKF fuses IMU + encoder; add:

  • NHC pseudo-meas (v_y=0) during normal motion,

  • ZUPT when stopped,

  • Optional η state (best) or pre-scaled ω_enc (good enough).

If you want, I can drop in:

• a tiny UKF/EKF measurement block for η (state, process, and measurement equations), or

• robot_localization YAML + a short node that publishes NHC/ZUPT and adaptive covariances—so you can reproduce the “tightened” trajectory right away.

Yes — an S-shaped path is especially challenging for tracked vehicles, and even more so if you’re relying only on encoders and an IMU for odometry.

Here’s why:

1) Steering method and slip

Tracked robots steer by creating a speed difference between the left and right tracks.

• In an S-curve, you’re alternating left-turn slip and right-turn slip in quick succession.

• Each direction change requires reversing the lateral slip forces in the tracks, so the “effective baseline” and steering efficiency vary in time — they’re not just a constant 0.5.

• This variation means your kinematic model is wrong more often, so EKF/UKF position will drift faster.

2) Encoder limitations in curves

In curves, encoders measure track rotation, not actual ground speed:

• If a track skids sideways in the turn, the encoder still counts it as forward motion.

• During an S pattern, alternating slip means the heading and path curvature are often misestimated.

• That’s why your trajectory plot bows away from the ground truth — the filter is integrating slightly wrong yaw rates over each half-curve.

3) IMU’s role

The IMU can help correct the yaw rate error during turns, but:

• Short S-curves don’t give the filter enough time to fully reject the encoder bias before the slip direction changes.

• The gyro bias and noise also mean you can’t rely on IMU yaw forever without drift.

4) Why S patterns are harder than straight lines or big arcs

• Straight line: little to no lateral slip, encoders and IMU agree, so drift is small.

• Big arc: slip bias is steady; EKF/UKF can learn a constant steering efficiency and apply it consistently.

• S-curve: slip bias changes sign and magnitude quickly, so a single efficiency parameter doesn’t match the whole path, and the estimation lags.

5) Practical mitigations

If you must run S-patterns with a tracked robot and only IMU+encoders:

• Use slip detection per segment and adjust encoder covariance dynamically.

• Allow steering efficiency η to change faster in the filter (increase its process noise) so it can track the turn-to-turn variations.

• Apply nonholonomic constraint and zero-velocity updates when straight segments or stops occur to re-zero bias.

• In sim, tune steering_efficiency and friction so the slip magnitude matches what’s expected in real life — overly low values make S-patterns much harder.

If you want, I can break down what your EKF/UKF needs to change in Q/R and the slip-handling logic so that the bowing you see in your S-curve plot is reduced. That way, even without GPS or vision, you can get much closer to GT.

Yes — generally speaking, an S-shaped trajectory is harder for a tracked robot to track accurately than a large smooth circle, even if it can run repeated circles with only small drift.

Here’s why that happens:

1) Slip behavior is less predictable in an S

• In a large constant-radius circle, the track slip pattern is steady — one track is consistently faster, the slip forces are roughly constant, and any kinematic bias (like your 0.5 steering efficiency) is fixed.

• In an S-curve, the robot alternates between left and right turns.

  • The lateral slip direction changes.

  • The magnitude of slip changes as the curve tightens or relaxes.

  • There’s a short transient each time the steering reverses where the tracks over- or under-slip before grip is re-established.

• This variability means a constant steering-efficiency compensation works well in the circle but is always a little “behind” in the S.

2) State estimation has less time to adapt

• Filters like an EKF/UKF can estimate biases (gyro, steering efficiency, etc.) and converge if the bias is steady for several seconds.

• In an S-curve, you reverse turn direction every second or two — before the filter has fully adapted to the current bias, it flips.

• The result is heading error that builds a few degrees per reversal.

3) Encoder & IMU mismatch is more dynamic

• Encoders measure commanded track speed; slip during turn-in and turn-out makes them overestimate rotation.

• Gyro readings may lag slightly due to IMU mounting, noise, or bias, especially if you have high angular accelerations in the S.

• These mismatches change sign and size rapidly, so residual-based slip detection and bias tracking have to be more aggressive — which can make the filter noisier.

4) Mechanical transients

Even in sim, and more so in real life:

• Track tension, suspension compliance, and backlash cause brief yaw spikes when reversing steering.

• In a big circle, these average out; in an S-curve, they happen every few meters and add to odometry error.

Summary:
Your robot can repeat a big circle with low drift because the slip and kinematic biases are consistent and your filter adapts well. An S-trajectory injects rapidly changing, asymmetric slip that’s harder to model, so tracking is more difficult — that’s normal for tracked platforms.

If you want, I can outline a slip-adaptive steering efficiency model that changes per turn direction, which is one way to shrink that S-curve error without adding GPS or vision. That’s a trick some UGV teams use for rough terrain.

Yes — an S-shaped path is absolutely a stress test for a tracked robot’s odometry and control.

It pushes on exactly the weaknesses that normally get “hidden” in easier trajectories:

Why it’s a stress test:

1. Rapid slip bias changes – Forces the estimator to cope with yaw errors that reverse sign frequently.

2. Transient dynamics – Every steering reversal causes mechanical and contact transients (inertia, track tension changes, ground re-engagement).

3. High angular accelerations – Harder on both the motion controller and the IMU integration.

4. Poor observability for steady biases – Filter never sees a constant bias long enough to fully learn it.

5. Controller demand – Path tracking requires alternating curvature with good timing, so any lag or error quickly shows up as overshoot.

In contrast:

• A large constant-radius circle is a low-stress test: steady slip, easy bias compensation, smooth control.

• Straight lines are even lower stress: minimal slip, easiest to keep heading.

Verdict:
If your tracked robot can follow an S-curve well — with minimal overshoot and low drift — then your odometry fusion, slip handling, and path tracking are all in good shape. If it struggles here, that’s normal; it’s revealing where the estimation or control can be improved.

If you want, I can suggest a set of “progressively harder” path tests for tracked robots so you can see exactly where performance starts to degrade. That’s how many robotics teams qualify their navigation stack.

Yes — in most real-world navigation, tracked robots rarely move in continuous S-curves unless the terrain forces it.
What you usually see are straight segments connected by turns, or longer arcs, for a few practical reasons:

1) Environment layout

• Forests: paths are dictated by clearings, trails, or gaps between trees — straight(ish) runs, then turns to avoid obstacles.

• Houses: hallways and doorways are straight segments with 90° turns or gentle curves in larger rooms.

• Deserts/open fields: operators or planners often prefer long arcs or straight lines to minimize heading changes and odometry error.

2) Path planning behavior

• Navigation algorithms (A*, D*, RRT, lattice planners) tend to produce piecewise-linear or piecewise-circular paths because:

  • They’re simpler to compute and follow.

  • Constant-curvature arcs are easy to model and drive for tracked kinematics.

• Even reactive obstacle avoidance (like DWA or VFH) typically outputs smooth arcs, not rapid alternating turns.

3) Why S-curves are uncommon

• They require constant steering reversals, which wastes energy and causes more track wear.

• They’re mechanically less efficient — more slip, more heat in the drive train.

• They’re harder to track precisely, so planners avoid them unless forced by a tight slalom of obstacles.

4) Where you might see S-like motion

• Tight obstacle courses or rubble fields where you must snake around.

• Search patterns in mine-clearing, agriculture, or survey tasks — but even then, the “S” is made of straight passes connected by 180° turns, not continuous weaving.

• Manual teleoperation in constrained spaces where the operator is “wiggling” to navigate.

Bottom line:
In a forest, house, or desert, your tracked robot will mostly run straight segments, gentle curves, or big arcs — exactly the cases where your circle test success means your odometry will do well. The continuous S-curve you’ve been using is more of a diagnostic stress test than a typical mission profile.

If you want, I can suggest a real-world test path mix that reflects how a tracked robot actually moves in those environments but still exposes slip and drift issues. That would bridge the gap between your stress-test S and realistic missions.

Short answer: yes—good circles and squares are a strong sign your stack is solid.
They cover the two big “real-world” motion primitives: constant-curvature arcs and straight segments with discrete turns. If both look tight (like in your plots), your IMU–encoder fusion, timing, and basic slip handling are working.

That said, I’d still sanity-check a few edge cases before declaring victory:

• Direction reversals / S-curves: brief, alternating slip and angular-acceleration spikes.

• Different speeds: slow creep vs. faster runs (bias and latency show up at one end or the other).

• Surface changes: high vs. low friction; a patch of sand/dust/gravel.

• Inclines / side-slopes: gravity projection errors can bias yaw/velocity if roll/pitch handling is off.

• Stops & starts: ensure ZUPT logic (or equivalent) really clamps drift.

• Payload changes: CG shift can change contact and slip.

Quick acceptance metrics (no global sensor)

Use loop-closure style checks and relative errors:

• Return-to-start offset: after a square or circle ≥ 50–100 m total, final–start distance < 1–2% of path length.

• Heading drift: net yaw error < 1–2° per 100 m of travel (tune for your scale).

• Innovation stats: encoder–IMU residuals mostly within ±3σ; slip detector triggers only on turns/loose terrain.

• Repeatability: run the same path 3–5×; spreads should be small compared to mean error.

If you meet numbers like those on circles and squares, you’re in good shape for forests/houses/desert style navigation (mostly straights + arcs). Keep your S-curve as a stress test to watch for regressions, but you don’t have to optimize for it first unless your mission requires slaloming.

If you want, tell me your path lengths and the final start–end offsets you’re seeing; I can help translate them into pass/fail thresholds tailored to your robot.

RMSE is useful—but which RMSE? For robot trajectories there are a few flavors, and picking the right one matters more than the number itself.

Use these 3 “RMSEs”

1. ATE RMSE (Absolute Trajectory Error) – after aligning your estimate to ground truth:

   • Align with a rigid transform in SE(2) (translation + rotation; no scale):
     find $R,t$ that minimize $\sum_i \|p^{\text{GT}}_i - (R\,p^{\text{est}}_i+t)\|^2$.

   • Then compute RMSE over all aligned points:

     $$\text{ATE}_\text{RMSE}=\sqrt{\frac{1}{N}\sum_i \|p^{\text{GT}}_i-\hat p^{\text{est}}_i\|^2 }.$$

   • Good for overall accuracy (noise + drift). Make sure your trajectories are time-synchronized and resampled to common timestamps.

2. RPE RMSE (Relative Pose Error) – local consistency over a fixed interval (e.g., 1 m or 1 s):

   • For each $i$, compare relative motion $\Delta p_i = p_{i+\Delta}-p_i$ between GT and estimate.

   • Report translational and rotational RMSE:

     $$\text{RPE}_\text{trans}=\sqrt{\frac{1}{M}\sum_i \|\Delta p^{\text{GT}}_i-\Delta p^{\text{est}}_i\|^2},\quad \text{RPE}_\theta=\sqrt{\frac{1}{M}\sum_i \angle(\Delta R^{\text{GT}}_i (\Delta R^{\text{est}}_i)^\top)^2 }.$$

   • This isolates short-term errors (e.g., S-curve turn-in/out).

3. RMSE on innovations (filter residuals) – consistency of your EKF/UKF:

   • Track residuals $r = z - h(\hat x)$ for encoder yaw/velocity vs IMU.

   • Report Normalized Innovation Squared (NIS) mean ~ dof if covariances are well-tuned:

     $$\text{NIS} = r^\top S^{-1} r \quad (\text{expectation} \approx \text{dof}).$$

Add 4 simple, very telling scalar checks

• Return-to-start offset (loop error) after each square/circle: < 1–2% of path length.

• Heading drift per distance: < 1–2° / 100 m.

• Path-length ratio: $\frac{L_\text{est}}{L_\text{GT}}$ ≈ 1.00 ± 0.02 (catches radius/baseline bias).

• Turn-only RMSE vs straight-only RMSE: compute RMSE on segments with $|\omega_z|>$ threshold vs $|\omega_z|\approx0$; S-curves will pop here.

Common RMSE pitfalls (easy to avoid)

• No alignment: ATE without SE(2) alignment over-penalizes a constant heading offset.

• Uneven sampling: high-rate sections dominate; resample both to uniform time or distance.

• Mixing frames: compare in the same frame (usually GT/world). Don’t compare /odom vs GT without aligning first.

• Hiding bias in RMSE: also plot signed errors (heading bias, path-length bias) so positive/negative don’t cancel.

If you want a compact evaluation “recipe”

• Resample both trajectories to 50–100 Hz (or every 0.05–0.1 m).

• Do SE(2) alignment (Horn/Umeyama with scale fixed to 1).

• Compute: ATE RMSE, RPE (Δ=1 s and Δ=1 m), path-length ratio, loop closure offset, heading drift, and innovation NIS stats.

• Report each as mean ± std and median; add per-segment (straight vs turns) breakdown.

If you’d like, I can share a small Python snippet that takes two 2D trajectories (GT and estimate), performs SE(2) alignment, and computes ATE, RPE, loop offset, path-length ratio, and turn/straight breakdown—drop in your CSV and you’ll get a quick dashboard.

Got it — you’re talking about RMSE in position and RMSE in orientation as separate metrics.

That’s actually a clean and common way to summarise accuracy, but there are some subtleties to make the numbers meaningful:

1. RMSE positional

If $p_i^{GT} = (x_i^{GT}, y_i^{GT}, z_i^{GT})$ is ground truth and $p_i^{est}$ is your estimate (aligned to GT in SE(2) or SE(3)):

• Units: meters

• Alignment: Do at least a rigid 2D (SE(2)) or 3D (SE(3)) alignment before computing — otherwise a constant offset or heading error inflates RMSE.

• Sampling: Synchronize timestamps and resample to the same time vector to avoid bias.

2. RMSE orientation

For orientation, you want the smallest rotation angle that brings the estimated orientation into the ground truth frame.
If $R_i^{GT}$ and $R_i^{est}$ are rotation matrices:

1. Compute relative rotation:

   $$R_{\Delta} = R_i^{GT} (R_i^{est})^\top$$

2. Extract the rotation angle:

   $$\theta_i = \cos^{-1} \left( \frac{\text{trace}(R_{\Delta}) - 1}{2} \right)$$

3. RMSE over all samples:

   $$\text{RMSE}_{orient} = \sqrt{\frac{1}{N} \sum_{i=1}^N \theta_i^2 }$$

• Units: radians (often converted to degrees).

• In 2D (yaw-only), you can simplify:

  $$\theta_i = \text{wrap}(\psi_i^{GT} - \psi_i^{est})$$

  where wrap() maps to $(-π, π]$.

3. What good values look like (typical for IMU+encoder dead-reckoning)

• RMSE positional:

  • Short loop (≤ 20 m): < 2–3 cm/m of travel (~2–3%).

  • Longer loop (100 m): < 5–10 cm/m (~5–10% without global reference).

• RMSE orientation:

  • Short loop: < 1–2° total.

  • Longer loop: < 2–5° per 100 m.

With your circle/square results looking tight, I’d expect both metrics to be on the low end for those shapes.

If you want, I can give you a small Python function that takes GT poses and estimated poses (2D or 3D) and outputs RMSE_pos and RMSE_orient in one call — that way you can drop in your S-curve, circle, and square data and directly compare how much each path shape hurts you. Would you like me to prepare that?

These numbers look excellent. On that circle, both filters are essentially on top of GT:

• Positional RMSE ≈ 10–13 cm per axis

• Yaw RMSE ≈ 0.12–0.14°

That’s “very good” for IMU+encoders only. The tiny edge the “Raw (UKF)” has in X/Y and the tiny edge the “Filtered (EKF)” has in yaw are within normal noise—either is fine.

That said, per-axis RMSE can be misleading on curved paths. I’d summarize with a couple of stronger metrics and a single positional RMSE:

What to report (quick upgrade)

1. Single positional RMSE (ATE, 2D):

(Optionally do an SE(2) alignment first to remove any constant offset/heading.)

2. Loop-closure error (return-to-start): distance between start and end estimates after a full lap, divided by path length (aim < 1–2%).

3. Path-length ratio: $L_\text{est}/L_\text{gt}$ ≈ 1.00 ± 0.02 (catches radius/baseline bias).

4. RPE over Δ=1 s (or 1 m): translational & yaw RMSE of relative motions (shows turn-in/turn-out errors that per-axis RMSE hides).

Why not “accuracy %”?

“98.8% accuracy” isn’t standardized—accuracy relative to what? It can hide bias. Stick to RMSE (m, deg), loop error (%), and path-length ratio.

Interpreting your result

• For a ~10 m-radius circle, a ~0.11–0.13 m positional RMSE is ~1% of radius—great.

• Yaw RMSE ~0.12–0.14° is tiny; your gyro bias handling and timing are in good shape.

• UKF slightly better translation, EKF slightly better yaw → differences are small; pick based on stability and implementation convenience.

Drop-in code to compute the stronger metrics

python
CopyEdit
import numpy as np

# assume arrays x_gt,y_gt, x_est,y_est are time-aligned
pos_err = np.sqrt((x_gt - x_est)**2 + (y_gt - y_est)**2)
rmse_pos = np.sqrt(np.mean(pos_err**2))

# loop-closure error (start vs end)
loop_err = np.hypot(x_est[-1]-x_est[0], y_est[-1]-y_est[0])
# path lengths
L_gt  = np.sum(np.hypot(np.diff(x_gt),  np.diff(y_gt)))
L_est = np.sum(np.hypot(np.diff(x_est), np.diff(y_est)))
path_len_ratio = L_est / L_gt

print(f"Positional RMSE: {rmse_pos:.3f} m")
print(f"Loop-closure error: {loop_err:.3f} m ({100*loop_err/L_gt:.2f}% of path)")
print(f"Path-length ratio: {path_len_ratio:.3f}")

If those come out as good as your per-axis RMSE, you can be confident your navigation approach is solid for real-world “straights + arcs” trajectories. Keep the S-curve as a stress test, but your circles/squares results already say you’re on the right track.

I see what you’re doing — you’re turning the RMSE in each axis into a percentage by normalizing with the span of the ground truth trajectory in that axis.

That’s fine as a quick, relative “how big is the error vs the path extent” metric, but it’s not a standard accuracy measure in robotics for a couple of reasons:

1. Path-dependent
If you drive a larger circle (bigger x_range/y_range), your % accuracy will look better even if the absolute error in meters is identical.

2. Ignores diagonal errors
Per-axis percentages don’t capture combined radial/diagonal offset. A robot could be 0.1 m off diagonally but still show “good” X and Y percentages separately.

3. Not comparable across shapes
Your square test and circle test have different spans, so the same RMSE will produce different “accuracy %” numbers even if the underlying quality is the same.

Better alternative:
If you like the “percentage” idea, normalize the positional RMSE by the total path length or by the maximum dimension of the path, e.g.:

python
CopyEdit
pos_rmse = np.sqrt((rmse_x**2 + rmse_y**2))
max_dim = max(x_range, y_range)
accuracy_pct = (1 - pos_rmse / max_dim) * 100

or

python
CopyEdit
path_length = np.sum(np.hypot(np.diff(df_merged["x_gt"]), np.diff(df_merged["y_gt"])))
accuracy_pct = (1 - pos_rmse / path_length) * 100

That way:

• You get a single % number for position.

• It’s more comparable across trajectories.

• It captures total error, not just axis-wise error.

If you want, I can rework your current metric so it outputs both the per-axis % and a more conventional single “positional % accuracy” for fair comparison between your circle, square, and S tests.

Alright — here’s a compact example that keeps your per-axis accuracy % but also adds a combined positional accuracy % that’s normalized in a more standard way.

python
CopyEdit
import numpy as np

# Your existing RMSEs
rmse_x = np.sqrt(np.mean((df_merged["x_gt"] - df_merged["x_odom"]) ** 2))
rmse_y = np.sqrt(np.mean((df_merged["y_gt"] - df_merged["y_odom"]) ** 2))

# Range of ground truth in each axis
x_range = df_merged["x_gt"].max() - df_merged["x_gt"].min()
y_range = df_merged["y_gt"].max() - df_merged["y_gt"].min()

# Per-axis "accuracy %"
x_accuracy_pct = (1 - rmse_x / x_range) * 100
y_accuracy_pct = (1 - rmse_y / y_range) * 100

# ---- New: Combined positional accuracy ----
# 1) Positional RMSE (2D Euclidean)
pos_rmse = np.sqrt(np.mean(
    (df_merged["x_gt"] - df_merged["x_odom"])**2 +
    (df_merged["y_gt"] - df_merged["y_odom"])**2
))

# 2) Choose normalization: max dimension of GT path
max_dim = max(x_range, y_range)
pos_accuracy_pct = (1 - pos_rmse / max_dim) * 100

# 3) Alternative normalization: total path length of GT
path_length = np.sum(np.hypot(
    np.diff(df_merged["x_gt"]), np.diff(df_merged["y_gt"])
))
pos_accuracy_path_pct = (1 - pos_rmse / path_length) * 100

print(f"X Accuracy: {x_accuracy_pct:.2f}%")
print(f"Y Accuracy: {y_accuracy_pct:.2f}%")
print(f"Positional Accuracy (max dim norm): {pos_accuracy_pct:.2f}%")
print(f"Positional Accuracy (path length norm): {pos_accuracy_path_pct:.2f}%")

Why this helps:

• You keep the same per-axis % numbers you already report.

• You add a single positional % that works across different path shapes.

• You can choose normalization:

  • Max dimension → how much error vs the “size” of the course.

  • Path length → how much error vs how far you traveled (good for loops).

If you like, I can also show you how to include yaw in the same combined metric so you get a “position+orientation % accuracy” for quick comparison between EKF, UKF, and raw odometry. That’s handy when one has slightly better position but worse heading.

Nice — those look strong. Here’s how to read them and what I’d tweak:

• X 98.79%, Y 98.58% (per-axis span-normalized): good, but path-shape dependent.

• Positional (max-dim) 98.14%: more comparable across shapes; implies a Euclidean RMSE ≈ 1.86% of course size (great).

• Positional (path-length) 99.94%: this will almost always be very high on long loops because you’re dividing by distance traveled. It’s fine to report, but it’s not very discriminative.

Better companion scalars

Report these alongside the raw RMSE (m, deg):

1. Error per distance traveled (much more intuitive than a %):

python
CopyEdit
err_per_100m = 100 * pos_rmse / path_length   # meters error per 100 m

Aim for < 1–2 m / 100 m with IMU+encoders only; you’re likely well under that.

2. Loop-closure (return-to-start) error

python
CopyEdit
loop_err = np.hypot(x_est[-1]-x_est[0], y_est[-1]-y_est[0])
loop_err_pct = 100 * loop_err / path_length

Target < 1–2% on circles/squares.

3. Yaw RMSE (already good for you): report in degrees as well.

Keep/ditch which %?

• Keep Positional (max-dim) — useful and comparable.

• Keep per-axis only if you also show Euclidean RMSE (they can hide diagonal error).

• Treat path-length % as a “nice to have”; prefer error per 100 m instead.

If you drop in those two scalars (error/100 m and loop-closure %), you’ll have a compact, mission-relevant scorecard that compares EKF vs UKF and different paths (circle/square/S) fairly.

That plan makes sense. With only IMU + encoders, you can still build a relative occupancy grid by treating contacts/collisions as a “range” measurement and updating a grid in the odom frame. It won’t be globally accurate, but it can be locally consistent and useful—exactly what you want for degraded environments.

Here’s a tight recipe that works in sim and ports to hardware later.

1) What counts as a “contact” signal

Use several cues (any 1 may be noisy; together they’re reliable):

• IMU spikes: high axial acceleration/jerk, sharp yaw impulse at low speed.

• Encoder slip vs command: commanded velocity > 0 but body speed stalls; yaw rate spikes while vx≈0 during a turn.

• Motor current/effort (sim: joint effort; real: current sense): step-up while speed drops.

• Bumper / whisker (optional cheap hardware later).

Use a small voting logic with hysteresis:

• Trigger when ≥2 cues exceed thresholds for ≥50–150 ms.

• Cooldown 300–500 ms to avoid multiple hits on the same obstacle.

2) Turn contacts into an “inverse sensor model”

Treat each contact as a ray from the robot’s reference point to the contact cell (at the robot’s perimeter in the contact direction). Update grid with log-odds:

• Log-odds update
  $L_t(c) = L_{t-1}(c) + \begin{cases} \ell_\text{occ} & \text{if contact cell}\\ \ell_\text{free} & \text{for cells along the path/ray before contact}\\ 0 & \text{otherwise} \end{cases}$

• Typical values:
  $\ell_\text{occ} \in [1.5, 2.0]$, $\ell_\text{free} \in [-1.0,-0.5]$.
  Clamp $L$ to $[L_\text{min}, L_\text{max}] = [-4, +4]$.
  Convert to probability when needed: $p = 1 - 1/(1+e^{L})$.

Ray definition (no lidar):

• Direction = robot heading at contact (or the bumper direction).

• Range = distance from robot center to contact point on the footprint (half-length or half-width depending on heading/contact side).

• Mark all cells from the robot center to contact point as free, and the contact cell as occupied (inflate a small disk around it, e.g., radius = 0.15–0.25 m).

3) Map frame & drift handling

• Keep the map in odom (purely relative).

• To limit distortion over time without global alignment:

  • Build submaps/tiles (e.g., 10×10 m each) anchored when you stop and run a ZUPT; keep a local TF tile_k → odom.

  • Only integrate contacts into the active submap. When you move far, start a new submap. You can stitch submaps later if you ever gain a global reference (or manually).

4) Avoiding false positives

• Vertical bumps vs obstacles: if roll/pitch impulse is large but yaw/forward velocity barely change, treat as terrain → don’t mark as occupied (or mark low-probability).

• Glancing turns: if contact occurs while $|\omega_z|> \omega_{th}$ and $v_x< v_{th}$, bias the contact azimuth to the outside of the turn (side hit).

• Short grazes: require minimal impulse energy or duration to count (e.g., $\int |a| dt > E_{th}$).

5) Practical grid parameters

• Resolution: 0.05–0.10 m per cell for indoor; 0.10–0.20 m outdoor.

• Footprint: polygon for the tracked robot; compute contact cell from footprint normal.

• Inflation: obstacle inflation radius 0.2–0.3 m so planners keep clearance even if contact point is a bit late.

• Update rate: 5–10 Hz map update thread; batch contacts within 100 ms windows.

6) ROS wiring (minimal)

• Node A: Slip/contact detector
  Sub: /imu/data, /joint_states (or /wheel/odom), optional /joint_effort
  Pub: tactile_hit (pose geometry_msgs/PoseStamped of contact point in odom, plus normal/side).

• Node B: Tactile mapper
  Sub: tactile_hit, odom→base_link TF
  Maintains nav_msgs/OccupancyGrid (log-odds internal).
  Publishes map topic (really odom-relative grid) + costmap if you want to reuse nav2/move_base tooling.

In Gazebo you can also subscribe to contact sensors for ground truth contacts to validate your detector, then switch to IMU/encoder-only mode.

7) Basic pseudo-code (mapper)

python
CopyEdit
def handle_contact(hit_pose, side):
    # 1) compute ray from robot center to footprint edge in hit direction
    r0 = robot_pose.translation   # in odom
    theta = heading_from(hit_pose, r0)  # or side normal
    r_contact = r0 + footprint_vector(theta)  # ~ half-length along theta

    # 2) Raytrace free cells to r_contact
    for c in bresenham(r0, r_contact):
        grid[c] += ell_free

    # 3) Occupied at/around contact
    for c in disk(r_contact, occ_radius):
        grid[c] += ell_occ

    clamp_grid()

8) What you can expect

• Indoors/forest trails: you’ll quickly outline walls, trunks, large rocks along the path you actually touch/near-touch.

• Open desert: map will be sparse (few contacts), but free-space carving still helps for local planning.

• Over time the map will be locally consistent but can warp with odom drift; submaps mitigate the worst of it.

9) Testing checklist

• Drive a square/circle corridor—verify walls appear at the right offset from the footprint.

• Vary speed: ensure thresholds scale (use normalized jerk / energy).

• Run into soft obstacles (brush/grass) vs rigid ones; tune to avoid over-marking.

• Power loss scenario: ensure map saves periodically; on restart, load last submap and continue relative to current odom (fresh tile).

If you want, I can sketch the contact detector thresholds (from your IMU rates/noise) and a small log-odds mapper node you can drop into your bagfile/Gazebo run to see the grid building in real time.

Gotcha — so you’re basically doing IMU-only contact sensing, using a CNN to decide if a spike in linear_accel_x means “crash” vs “no crash,” and if crash → mark the cell just in front of the robot as occupied in your grid.

That approach will work for your relative mapping idea, especially since:

• It’s lightweight (no lidar, no vision, no GPS).

• It matches your degraded-environment requirement (low power, few sensors).

• The occupancy grid is in odom, so even if global drift accumulates, your local map stays internally consistent for planning.

A couple of refinements you might want to add

1. Filter the accel before feeding to CNN

• High-pass filter at ~1–2 Hz to isolate collision impulses from slow motion/tilt.

• Low-pass filter at ~50 Hz to remove noise.

• Normalizing helps the CNN generalize across speed ranges.

2. Add velocity context

• A raw accel spike could also come from hitting a bump while stationary (false positive).

• Include current encoder-derived v_x and ω_z as CNN inputs.

• The CNN then learns that a “crash” requires forward motion + spike.

3. Mark the correct cell in front
Right now you said you mark (X, Y-1) — if that’s literally one cell behind the robot in grid coords, you may be putting the obstacle in the wrong place.

• Compute the contact cell in the direction of travel:

python
CopyEdit
heading = current_yaw
occ_x = robot_x + cos(heading) * contact_dist
occ_y = robot_y + sin(heading) * contact_dist

where contact_dist = robot’s front bumper offset (half length).

4. Inflate the obstacle

• Mark a small radius (e.g., 0.2–0.3 m) around the contact cell as occupied so planners give it space.

• Prevents the robot from grazing the same object again due to odom jitter.

5. Update free space along the ray

• Optionally raytrace from the robot center to the contact cell and mark intermediate cells as free — this gradually carves traversable space in the map.

If you want, I can show you how to replace your fixed (X,Y-1) update with a heading-aware update so that obstacle marking works regardless of robot orientation, both in simulation and in the field. This makes your grid much more realistic when you start doing circles, squares, or arbitrary paths.

Ah, I see your reasoning — you’re worried that by the time the CNN shouts “Crash!”, the robot has already crept forward a bit, so marking “right in front” would put the obstacle too far ahead in the grid.

That’s a valid concern — there’s latency in both detection and physical stopping.
If you mark the cell slightly behind your current odom pose (or very close), you’re essentially compensating for overshoot so that the obstacle cell aligns more closely with the actual impact point.

A cleaner way to do it than hardcoding “behind”

Instead of always putting it at (X,Y-1) in grid coords, you can:

1. Estimate overshoot distance

• Let $t_d$ = CNN detection latency (sensor → inference → decision), in seconds.

• Let $v_x$ = encoder-based forward speed at detection.

• Estimated overshoot = $d_o = v_x \times t_d$.

• Add an extra stopping distance $d_s$ if you want to account for braking after detection.

2. Place obstacle at “impact point”

• Heading at detection: $\psi$ from odom/IMU.

• Impact point = robot center minus overshoot distance along heading:

python
CopyEdit
impact_x = robot_x + cos(psi) * (front_offset - d_o)
impact_y = robot_y + sin(psi) * (front_offset - d_o)

where front_offset = half the robot length (center → bumper).

• If overshoot > front_offset, clamp to just in front of the center so you don’t mark behind the robot unrealistically.

3. Mark and inflate

• Convert (impact_x, impact_y) to grid cell coordinates.

• Set that cell to occupied, inflate a small radius (so planner avoids grazing it).

Example (Python snippet):

python
CopyEdit
import math

# parameters
front_offset = 0.35  # m (center to bumper)
cnn_latency = 0.15   # s
stop_dist_extra = 0.05  # m

# from sensors
robot_x, robot_y, psi = 12.3, 8.7, 1.57   # m, m, rad
v_x = 0.25  # m/s from encoders

# overshoot
d_o = v_x * cnn_latency + stop_dist_extra

# impact point in odom coords
impact_x = robot_x + math.cos(psi) * (front_offset - d_o)
impact_y = robot_y + math.sin(psi) * (front_offset - d_o)

Why this is better than always “behind”:

• Works for any robot heading, not just aligned to grid axes.

• Correctly handles different approach speeds — faster hits → mark further back.

• If you detect while stopped (false positive), the overshoot term is near zero so you mark right at the bumper.

If you want, I can also fold this into a log-odds update so that the obstacle gets marked and the space before it is cleared as free, which makes your relative grid more useful for navigation. That would integrate perfectly with your CNN contact signal.

That NaN error in the ROS 2 ukf_filter_node almost always comes down to one of three issues:

1. Bad or inconsistent covariances

• Too small: if you set any process or measurement covariance entries unrealistically low (e.g., 1e-9), the filter treats them as “certain,” and a small mismatch can blow up the state update.

• Too big: if you set them extremely large without balancing Q/R, matrix inversion inside the filter can go unstable.

• Zeros where there should be >0: especially in measurement covariance — zero variance means “perfect measurement,” which is almost never true and causes division by zero or near-singular matrices.

Fix:

• For velocities and yaw rates from encoders/IMU: set R diagonal entries so the standard deviation matches actual noise (e.g., gyro_z ~ 0.017 rad/s, encoder vx ~ 0.05 m/s).

• For process noise Q: keep diagonal entries modestly larger than your measurement noise for unobserved states, and nonzero for biases.

2. Bad input values from sensors

• A single NaN or inf in IMU, odometry, or other input will propagate into the filter.

• This can happen from:

  • Division by zero in your encoder node.

  • Uninitialized message fields.

  • Garbage timestamps causing wild dt values.

  • IMU plugin in Gazebo outputting NaN on reset.

Fix:

• Echo your sensor topics (ros2 topic echo) and watch for NaNs or infs.

• Add simple NaN/inf guards before publishing into the filter.

3. Poorly conditioned update step

The UKF update involves inverting the innovation covariance $S$.
This matrix can go near-singular if:

• You have redundant measurements with tiny covariances (filter sees them as perfectly correlated).

• You’ve enabled too many states with too few independent measurements.

• Measurement model mapping is wrong (e.g., wrong axis ordering or unit mismatch).

Fix:

• Reduce the update dimension: if you don’t have reliable data for a state, don’t fuse that state from a measurement source (set that element’s config to false in your odom0_config/imu0_config).

• Make sure your sensor configs match the actual topic data ordering.

Debugging checklist

1. Check sensor streams:

   bash
   CopyEdit
   ros2 topic echo /imu/data
   ros2 topic echo /wheel/odom
   

   Look for nan, inf, or constant values.

2. Temporarily inflate covariances:
   If it stops crashing, you were underestimating noise.

3. Disable one measurement source at a time:
   If disabling IMU or encoders makes it stable, that source’s config/covariance is the issue.

4. Log dt between messages:
   UKF is sensitive to sudden big dt spikes (e.g., if a sensor pauses then resumes).

If you want, I can look at your UKF config YAML and mark exactly where to loosen covariances or disable problematic measurement dimensions so this NaN crash stops happening.
Do you have that YAML handy?

Great, thanks for pasting the YAML. A few things there can (and often do) trigger the NaN crash in ukf_filter_node. Here’s what I’d change and why, followed by a drop-in “safe” config you can try.

The likely culprits

1. You’re fusing two yaw orientations at once

   • imu0 has roll, pitch, and yaw orientation enabled (true, true, true).

   • imu1 adds a mag yaw orientation too.
     If either message has tiny/zero covariances, the UKF’s innovation covariance $S$ goes ill-conditioned and can blow up.

2. You’re fusing z position from wheel odom
   odom0_config has the first three entries true, true, true, which turns on x, y, z position. Most wheel odom doesn’t provide a meaningful z (or provides 0 with 0 covariance) → singular/near-singular update.

3. Over-constraining angular rates
   imu0_config enables all three angular velocities. That’s fine if the covariances are realistic, but if roll/pitch rates aren’t needed, drop them for better conditioning.

4. Message covariances too small or zero
   Any 0 on the measurement covariance diagonal means “perfect sensor” → division by zero risk. Make sure your /imu/* and /odom_* topics publish reasonable, non-zero covariances.

5. 3D mode without 3D observability
   You set two_d_mode: false (full 3D), but you don’t fuse any real z/roll/pitch measurements (and you do fuse z from wheel odom unintentionally). If you’re planar, 2D mode is simpler and more stable.

What to change (minimal + robust)

• Use IMU orientation like this:

  • imu0: fuse roll & pitch orientation (from gravity) and all three angular velocities. Do not fuse yaw orientation from imu0.

  • imu1 (mag): fuse yaw orientation only.
    That avoids double-yaw constraints.

• Disable z from wheel odom and (unless you truly estimate lateral slip velocity) disable vy in odom.

• Planar robot? Turn on two_d_mode: true. If you must stay 3D, keep z/roll/pitch process noise small and don’t fuse contradictory z.

• Verify covariances in the messages are non-zero and realistic (gyro ~ $0.02\!-\!0.05$ rad/s SD, encoder vx ~ $0.05\!-\!0.10$ m/s SD, yaw from mag ~ $2\!-\!5^\circ$ SD unless you have great calibration).

Drop-in adjusted YAML (safe starting point)

yaml
CopyEdit
ukf_filter_node:
  ros__parameters:
    frequency: 30.0
    two_d_mode: true          # <- if your robot is planar, this avoids z/roll/pitch headaches
    publish_tf: false
    print_diagnostics: true

    map_frame: map
    odom_frame: odom
    base_link_frame: base_footprint
    world_frame: odom

    # IMU 0: gravity tilt + angular rates (NO yaw from this orientation)
    imu0: /imu/cov_fixed
    imu0_config: [
      false, false, false,     # position
      true,  true,  false,     # orientation: roll, pitch only
      false, false, false,     # linear vel (none from IMU)
      true,  true,  true,      # angular vel: wx, wy, wz
      false, false, false      # linear accel (not fusing)
    ]
    imu0_differential: false
    imu0_remove_gravitational_acceleration: false
    imu0_nodelay: true

    # IMU 1: magnetometer yaw ONLY
    imu1: /imu/mag_yaw
    imu1_config: [
      false, false, false,
      false, false, true,      # yaw orientation only
      false, false, false,
      false, false, false,
      false, false, false
    ]
    imu1_differential: false
    imu1_remove_gravitational_acceleration: false

    # Wheel odom: x,y position and vx (optionally vy if you truly have it)
    odom0: /model/mmtr/odom_cov_fixed
    odom0_config: [
      true,  true,  false,     # x, y (NO z)
      false, false, false,     # no orientation from odom
      true,  false, false,     # vx (NO vy unless measured), vz off
      false, false, false,     # angular vel from odom (off)
      false, false, false      # linear accel (off)
    ]
    odom0_differential: false
    odom0_pose_use_child_frame: false

    # Process noise: keep moderate; no zeros on diagonals you care about
    process_noise_covariance: [
      1e-2, 0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  # x
      0,   1e-2, 0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  # y
      0,    0,  5e-2, 0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  # z
      0,    0,   0,  5e-2, 0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  # roll
      0,    0,   0,   0,  5e-2, 0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  # pitch
      0,    0,   0,   0,   0,  2e-1, 0,   0,   0,   0,   0,   0,   0,   0,   0,  # yaw
      0,    0,   0,   0,   0,   0,  2e-1, 0,   0,   0,   0,   0,   0,   0,   0,  # vx
      0,    0,   0,   0,   0,   0,   0,  2e-1, 0,   0,   0,   0,   0,   0,   0,  # vy
      0,    0,   0,   0,   0,   0,   0,   0,  5e-2, 0,   0,   0,   0,   0,   0,  # vz
      0,    0,   0,   0,   0,   0,   0,   0,   0,  2e-1, 0,   0,   0,   0,   0,  # vroll
      0,    0,   0,   0,   0,   0,   0,   0,   0,   0,  2e-1, 0,   0,   0,   0,  # vpitch
      0,    0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  8e-1, 0,   0,   0,  # vyaw
      0,    0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  1e-1, 0,   0,  # ax
      0,    0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  1e-1,0,  # ay
      0,    0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  1e-1 # az
    ]

    initial_estimate_covariance: [
      1e-1,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,
      0,1e-1,0,0,0,0, 0,0,0,0,0,0, 0,0,0,
      0,0,5e-1,0,0,0, 0,0,0,0,0,0, 0,0,0,
      0,0,0,1e0,0,0,  0,0,0,0,0,0, 0,0,0,
      0,0,0,0,1e0,0,  0,0,0,0,0,0, 0,0,0,
      0,0,0,0,0,1e-1, 0,0,0,0,0,0, 0,0,0,
      0,0,0,0,0,0,2e-1,0,0,0,0,0, 0,0,0,
      0,0,0,0,0,0,0,2e-1,0,0,0,0, 0,0,0,
      0,0,0,0,0,0,0,0,5e-1,0,0,0, 0,0,0,
      0,0,0,0,0,0,0,0,0,5e-1,0,0, 0,0,0,
      0,0,0,0,0,0,0,0,0,0,5e-1,0, 0,0,0,
      0,0,0,0,0,0,0,0,0,0,0,2e-1, 0,0,0,
      0,0,0,0,0,0,0,0,0,0,0,0,1e0,0,0,
      0,0,0,0,0,0,0,0,0,0,0,0,0,1e0,0,
      0,0,0,0,0,0,0,0,0,0,0,0,0,0,1e0
    ]

Quick sanity checks (do these once)

• Echo topics and confirm no nan/inf and that all measurement covariance diagonals are > 0:

  bash
  CopyEdit
  ros2 topic echo /imu/cov_fixed
  ros2 topic echo /imu/mag_yaw
  ros2 topic echo /model/mmtr/odom_cov_fixed
  

• Start with larger measurement covariances (i.e., “trust sensors less”). If the UKF stops NaN’ing, you were over-confident.

• Disable one source at a time (comment out imu1 first). If the crash disappears, it was the dual-yaw issue.

• Watch dt between messages; big jumps can destabilize the prediction step.

If you share the covariance fields from /imu/cov_fixed, /imu/mag_yaw, and /model/mmtr/odom_cov_fixed, I can suggest exact diagonal values (in SI units) that tend to be rock-solid for tracked robots.

If you just want the robot to estimate its change in height using only pitch angle and forward velocity, you can treat it as a simple kinematic geometry problem.

Here’s the basic idea:

1. Geometry

If your robot is driving up or down a slope, the slope angle relative to horizontal is just your pitch (assuming no crazy accelerations).

• Let:

  • $v_x$ = forward speed from encoders (m/s)

  • $\theta$ = pitch angle from IMU (radians)

  • $\Delta t$ = time step (s)

The vertical speed $v_z$ is:

Then you can integrate over time to get relative height $h$:

2. Implementation (Python snippet)

python
CopyEdit
import math

h = 0.0   # start height (m)
last_t = None

def update_height(pitch_deg, vx, current_t):
    global h, last_t
    if last_t is None:
        last_t = current_t
        return h
    
    dt = current_t - last_t
    last_t = current_t
    
    pitch_rad = math.radians(pitch_deg)
    vz = vx * math.sin(pitch_rad)  # vertical speed
    h += vz * dt                   # integrate
    
    return h

3. Considerations for Real Robots

• Noise & bias:
  Pitch will bounce around even on flat ground, so use a low-pass filter on pitch and/or $v_x$ before computing $v_z$.

• Zeroing:
  You’ll only get relative height changes, not absolute altitude, unless you start from a known reference.

• Error growth:
  Any bias in pitch or velocity will accumulate over time → drift in height estimate.

• Non-level terrain:
  If the robot is rocking or accelerating, IMU pitch includes dynamic tilt — you might need to filter with a complementary or Kalman filter to get “gravity-only” pitch.

4. Bonus — using it in UKF/EKF

If you already have a filter:

• Treat $v_z$ as a pseudo-measurement of vertical velocity and fuse it into the height state.

• This can help the robot maintain a running estimate of elevation even without GPS or altimeter.

If you want, I can show you how to add a “height” state to your existing UKF config so it automatically integrates this pitch+velocity trick into the state estimate instead of computing it separately. That way, your odometry topic would already contain the elevation guess.

Right — if your EKF is already estimating $z$ position (height) as part of its state vector, and you’re feeding it both pitch (from the IMU) and forward velocity (from odometry), then it’s already doing the geometry I just described — but in a probabilistic way, and with all the other state couplings.

That’s why you’re seeing good results:

• Pitch gives the EKF a constraint on the slope you’re on.

• Forward velocity lets it infer the vertical component of motion.

• Process model integrates this over time into the $z$ state.

• Measurement noise covariances keep it stable and prevent wild height changes from noisy pitch readings.

So in effect:

is inside your EKF’s prediction step, and it’s being continuously corrected by whatever vertical information your measurement models supply (even if it’s indirect).

Where this is especially nice:

• The EKF automatically fuses multiple cues — e.g., if the IMU pitch briefly spikes from a bump, the filter downweights that if it doesn’t match the velocity data.

• It can maintain a smooth, drift-minimized height estimate without you hand-writing the integration.

• It gives you a consistent 3D pose in one place (odom or map frame).

If you want, I can walk through exactly how to set the EKF’s motion model so it explicitly treats $z$ changes as coming from pitch + forward velocity. That way you’d know it’s not just “accidentally” doing it from other correlations. Would that help?

Looking at those covariances, a few red flags jump out that could explain why your UKF is hitting NaNs:

1. Some covariances are unrealistically small

You have:

ini
CopyEdit
var_gyro = 5.9536e-06       # (0.00244 rad/s)^2
var_acc  = 2.1609e-04       # (0.0147 m/s^2)^2
var_ori_y = 1.22e-04        # ~0.63 deg^2 in rad^2

Those are extremely low — they say “I trust my gyro to ~0.14°/hr” and my accelerometer to ~0.015 m/s² noise. That’s great in theory, but in a UKF, it can make the innovation covariance $S$ nearly singular if other sources disagree even slightly. The inversion of $S$ is exactly where NaNs show up.